#include "MPC_Base.h"
#include "MPC3.h"
#include "../../ThirdPartyCode/Utilities.h"
#include "../../ThirdPartyCode/NR_Vector_CVersion.h"
#include "../model.h"

// ----------------------------------------------------------------------
MPC3::MPC3( double	iDens_set,
			double	iFact_D,
			double	iRough2_set,
			double	iFact_R2,
			double	iHt_set,
			double	iFact_H, 
			double	it_end,
			double	iDepRate_ini,
			double	iRT_Dep,
			double	it_int_ctrl, 
			int		iStep_num,
			int		iRel_err,
			char*	iFileName,
			int		iDetail,
			int		iControllerID,
			char*   iMPCFunctionName):
			MPC_Base(iDens_set,
				     iFact_D,
					 iRough2_set,
					 iFact_R2,
					 iHt_set,
					 iFact_H,
					 it_end,
					 iDepRate_ini,
					 iRT_Dep,
					 it_int_ctrl,
					 iStep_num,
					 iRel_err,
					 iFileName,
					 iDetail,
					 iControllerID)
{
	if (!(ep = engOpen("\0"))) 
	{
		fprintf(stderr, "\nCan't start MATLAB engine\n");
		system("pause");
		exit(0);
	}
	engOutputBuffer(ep,MatlabOutputBuffer,5120);

	FlagResult = engEvalString(ep,"cd('C:/Documents and Settings/Xinyu/Desktop/LPCVD_Develop/Controller/Matlab_MPC')");
	engEvalString(ep,"addpath(genpath('C:/Documents and Settings/Xinyu/Desktop/LPCVD_Develop/Controller/Matlab_MPC'))");
	cout << MatlabOutputBuffer << "\n";
	engEvalString(ep,"clear all");
	engEvalString(ep,"diary MPC_Matlab.log");
	
	mxAlphas 	= mxCreateDoubleMatrix(MODE,1,mxREAL);
	alphas		= mxGetPr(mxAlphas);
	engPutVariable(ep,"Alphas",mxAlphas);

	mxBetas 	= mxCreateDoubleMatrix(MODE,1,mxREAL);
	betas		= mxGetPr(mxBetas);
	engPutVariable(ep,"Betas",mxBetas);

	mxHt 		= mxCreateDoubleScalar(-1.0);
	pHt			= mxGetPr(mxHt);
	engPutVariable(ep,"Ht",mxHt);

	mxLb		= mxCreateDoubleMatrix(Step_num,1,mxREAL);
	pLb			= mxGetPr(mxLb);
	engPutVariable(ep,"Lb",mxLb);

	mxUb		= mxCreateDoubleMatrix(Step_num,1,mxREAL);
	pUb			= mxGetPr(mxUb);
	engPutVariable(ep,"Ub",mxUb);

	mxG_dens 	= mxCreateDoubleScalar(-1.0);
	pG_dens		= mxGetPr(mxG_dens);
	engPutVariable(ep,"G_dens",mxG_dens);
	
	mxW0		= mxCreateDoubleScalar((DEP_LOW+DEP_HIGH)/2);
	pW0			= mxGetPr(mxW0);
	engPutVariable(ep,"W0",mxW0);

	mxTime		= mxCreateDoubleScalar(0);
	pTime		= mxGetPr(mxTime);
	engPutVariable(ep,"Time",mxTime);

	mxD_set		= mxCreateDoubleScalar(iDens_set);
	mxFact_D	= mxCreateDoubleScalar(iFact_D);
	mxR2_set	= mxCreateDoubleScalar(iRough2_set);
	mxFact_R2	= mxCreateDoubleScalar(iFact_R2);
	mxHt_set    = mxCreateDoubleScalar(iHt_set);
	mxFact_H    = mxCreateDoubleScalar(iFact_H);
	mxVarR2_set = mxCreateDoubleScalar(0.0);
	mxFact_VarR2= mxCreateDoubleScalar(0.0);
	mxStep_num	= mxCreateDoubleScalar(iStep_num);
	mxMode		= mxCreateDoubleScalar(MODE);
	mxdt		= mxCreateDoubleScalar(it_int_ctrl);
	mxDEP_LOW	= mxCreateDoubleScalar(DEP_LOW);
	mxDEP_HIGH	= mxCreateDoubleScalar(DEP_HIGH);
	mxRT_dep	= mxCreateDoubleScalar(iRT_Dep);
	mxT_end		= mxCreateDoubleScalar(it_end);
	mxControllerTypeID = mxCreateDoubleScalar(ControllerTypeID);

	engPutVariable(ep,"D_set",mxD_set);
	engPutVariable(ep,"Fact_D",mxFact_D);
	engPutVariable(ep,"R2_set",mxR2_set);
	engPutVariable(ep,"Fact_R2",mxFact_R2);
	engPutVariable(ep,"Ht_set",mxHt_set);
	engPutVariable(ep,"Fact_H",mxFact_H);
	engPutVariable(ep,"Step_num",mxStep_num);
	engPutVariable(ep,"Mode",mxMode);
	engPutVariable(ep,"dt",mxdt);
	engPutVariable(ep,"DEP_LOW",mxDEP_LOW);
	engPutVariable(ep,"DEP_HIGH",mxDEP_HIGH);
	engPutVariable(ep,"RT_dep",mxRT_dep);
	engPutVariable(ep,"T_end",mxT_end);
	engPutVariable(ep,"ControllerTypeID",mxControllerTypeID);

	engEvalString(ep,"Settings.D_set     = D_set");
	engEvalString(ep,"Settings.Fact_D    = Fact_D");
	engEvalString(ep,"Settings.R2_set    = R2_set");
	engEvalString(ep,"Settings.Fact_r2   = Fact_R2");
	engEvalString(ep,"Settings.Ht_set    = Ht_set");
	engEvalString(ep,"Settings.Fact_H    = Fact_H");
	engEvalString(ep,"Settings.P         = Step_num");
	engEvalString(ep,"Settings.m         = Mode");
	engEvalString(ep,"Settings.dt        = dt");
	engEvalString(ep,"Settings.lb        = DEP_LOW");
	engEvalString(ep,"Settings.ub        = DEP_HIGH");
	engEvalString(ep,"Settings.rt_dep    = RT_dep");
	engEvalString(ep,"Settings.T_end	 = T_end");
	engEvalString(ep,"Settings.ControllerTypeID = ControllerTypeID");

	MPCFunctionName = iMPCFunctionName;	
}
// ----------------------------------------------------------------------
MPC3::MPC3( double	iDens_set,
			double	iFact_D, 
			double	iRough2_set,
			double	iFact_R2, 
			double	iHt_set,	
			double	iFact_H, 
			double  ivarR2_set,
			double  iFact_varR2,
			double  iM2_set,
			double  iM2_fact,
			double	it_end,
			double	iDepRate_ini,
			double	iRT_Dep,
			double	it_int_ctrl, 
			int		iStep_num,
			int		iRel_err,
			char*	iFileName,
			int		iDetail,
			int		iControllerID,
			char*   iMPCFunctionName):
			MPC_Base(iDens_set,
				     iFact_D,
					 iRough2_set,
					 iFact_R2,
					 iHt_set,
					 iFact_H,
					 it_end,
					 iDepRate_ini,
					 iRT_Dep,
					 it_int_ctrl,
					 iStep_num,
					 iRel_err,
					 iFileName,
					 iDetail,
					 iControllerID)
{
	cout << "\n MPC using Matlab engine (MPC3) is used\n";
	
	if (!(ep = engOpen("\0"))) 
	{
		fprintf(stderr, "\nCan't start MATLAB engine\n");
		system("pause");
		exit(0);
	}
	engOutputBuffer(ep,MatlabOutputBuffer,5120);

	FlagResult = engEvalString(ep,"cd('C:/Documents and Settings/Xinyu/Desktop/LPCVD_Develop/Controller/Matlab_MPC')");
	engEvalString(ep,"addpath(genpath('C:/Documents and Settings/Xinyu/Desktop/LPCVD_Develop/Controller/Matlab_MPC'))");
	cout << MatlabOutputBuffer << "\n";
	engEvalString(ep,"clear all");
	engEvalString(ep,"diary MPC_Matlab.log");
	
	mxAlphas 	= mxCreateDoubleMatrix(MODE,1,mxREAL);
	alphas		= mxGetPr(mxAlphas);
	engPutVariable(ep,"Alphas",mxAlphas);

	mxBetas 	= mxCreateDoubleMatrix(MODE,1,mxREAL);
	betas		= mxGetPr(mxBetas);
	engPutVariable(ep,"Betas",mxBetas);

	mxHt 		= mxCreateDoubleScalar(-1.0);
	pHt			= mxGetPr(mxHt);
	engPutVariable(ep,"Ht",mxHt);

	mxLb		= mxCreateDoubleMatrix(Step_num,1,mxREAL);
	pLb			= mxGetPr(mxLb);
	engPutVariable(ep,"Lb",mxLb);

	mxUb		= mxCreateDoubleMatrix(Step_num,1,mxREAL);
	pUb			= mxGetPr(mxUb);
	engPutVariable(ep,"Ub",mxUb);

	mxG_dens 	= mxCreateDoubleScalar(-1.0);
	pG_dens		= mxGetPr(mxG_dens);
	engPutVariable(ep,"G_dens",mxG_dens);
	
	mxW0		= mxCreateDoubleScalar((DEP_LOW+DEP_HIGH)/2);
	pW0			= mxGetPr(mxW0);
	engPutVariable(ep,"W0",mxW0);

	mxTime		= mxCreateDoubleScalar(0);
	pTime		= mxGetPr(mxTime);
	engPutVariable(ep,"Time",mxTime);
	
	mxD_set		= mxCreateDoubleScalar(iDens_set);
	mxFact_D	= mxCreateDoubleScalar(iFact_D);
	mxR2_set	= mxCreateDoubleScalar(iRough2_set);
	mxFact_R2	= mxCreateDoubleScalar(iFact_R2);
	mxHt_set    = mxCreateDoubleScalar(iHt_set);
	mxFact_H    = mxCreateDoubleScalar(iFact_H);
	mxVarR2_set = mxCreateDoubleScalar(ivarR2_set);
	mxFact_VarR2= mxCreateDoubleScalar(iFact_varR2);
	mxM2_set    = mxCreateDoubleScalar(iM2_set);
	mxM2_fact   = mxCreateDoubleScalar(iM2_fact);
	mxStep_num	= mxCreateDoubleScalar(iStep_num);
	mxMode		= mxCreateDoubleScalar(MODE);
	mxdt		= mxCreateDoubleScalar(it_int_ctrl);
	mxDEP_LOW	= mxCreateDoubleScalar(DEP_LOW);
	mxDEP_HIGH	= mxCreateDoubleScalar(DEP_HIGH);
	mxRT_dep	= mxCreateDoubleScalar(iRT_Dep);
	mxT_end		= mxCreateDoubleScalar(it_end);
	mxControllerTypeID = mxCreateDoubleScalar(ControllerTypeID);
	
	engPutVariable(ep,"D_set",mxD_set);
	engPutVariable(ep,"Fact_D",mxFact_D);
	engPutVariable(ep,"R2_set",mxR2_set);
	engPutVariable(ep,"Fact_R2",mxFact_R2);
	engPutVariable(ep,"Ht_set",mxHt_set);
	engPutVariable(ep,"Fact_H",mxFact_H);
	engPutVariable(ep,"varR2_set",mxVarR2_set);
	engPutVariable(ep,"Fact_varR2",mxFact_VarR2);
	engPutVariable(ep,"M2_set",mxM2_set);
	engPutVariable(ep,"M2_fact",mxM2_fact);
	engPutVariable(ep,"Step_num",mxStep_num);
	engPutVariable(ep,"Mode",mxMode);
	engPutVariable(ep,"dt",mxdt);
	engPutVariable(ep,"DEP_LOW",mxDEP_LOW);
	engPutVariable(ep,"DEP_HIGH",mxDEP_HIGH);
	engPutVariable(ep,"RT_dep",mxRT_dep);
	engPutVariable(ep,"T_end",mxT_end);
	engPutVariable(ep,"ControllerTypeID",mxControllerTypeID);
	
	engEvalString(ep,"MPC1xx_ini");

	engEvalString(ep,"Settings.D_set     = D_set");
	engEvalString(ep,"Settings.Fact_D    = Fact_D");
	engEvalString(ep,"Settings.R2_set    = R2_set");
	engEvalString(ep,"Settings.Fact_r2   = Fact_R2");
	engEvalString(ep,"Settings.Ht_set    = Ht_set");
	engEvalString(ep,"Settings.Fact_H    = Fact_H");
	engEvalString(ep,"Settings.varR2_set = varR2_set");
	engEvalString(ep,"Settings.Fact_varR2= Fact_varR2");
	engEvalString(ep,"Settings.M2_set    = M2_set");
	engEvalString(ep,"Settings.M2_fact   = M2_fact");
	engEvalString(ep,"Settings.P         = Step_num");
	engEvalString(ep,"Settings.m         = Mode");
	engEvalString(ep,"Settings.dt        = dt");
	engEvalString(ep,"Settings.lb        = DEP_LOW");
	engEvalString(ep,"Settings.ub        = DEP_HIGH");
	engEvalString(ep,"Settings.rt_dep    = RT_dep");
	engEvalString(ep,"Settings.T_end	 = T_end");
	engEvalString(ep,"Settings.weight    = [1.0,0.8,0.6,0.4,0.2]");
	engEvalString(ep,"Settings.model     = ModelSettings");
	engEvalString(ep,"Settings.Input     = {'alpha','beta','h','rho'}");
	engEvalString(ep,"Settings.Output    = {'W(layer/s)'");
	engEvalString(ep,"Settings.ControllerTypeID = ControllerTypeID");

	MPCFunctionName = iMPCFunctionName;

	engEvalString(ep,"State.h = 0");
	engEvalString(ep,"State.rho = 0");
	engEvalString(ep,"State.meanAlpha2 = zeros(1,Settings.m)");
	engEvalString(ep,"State.meanBeta2 = zeros(1,Settings.m)");
	engEvalString(ep,"State.meanR2 = 0");
	engEvalString(ep,"State.varAlpha2 = zeros(1,Settings.m)");
	engEvalString(ep,"State.varBeta2  = zeros(1,Settings.m)");
	engEvalString(ep,"State.varR2     = zeros(1,Settings.m)");

	fout_MPC3.open("MPC3.log");
}
// ----------------------------------------------------------------------
MPC3::~MPC3()
{
	mxDestroyArray(mxAlphas);
	mxDestroyArray(mxBetas);
	mxDestroyArray(mxHt);
//	mxDestroyArray(mxRough);
	mxDestroyArray(mxG_dens);
	mxDestroyArray(mxW0);
	
	mxDestroyArray(mxD_set);
	mxDestroyArray(mxFact_D);
	mxDestroyArray(mxR2_set);
	mxDestroyArray(mxFact_R2);
	mxDestroyArray(mxHt_set);
	mxDestroyArray(mxFact_H);
	mxDestroyArray(mxVarR2_set);
	mxDestroyArray(mxFact_VarR2);
	mxDestroyArray(mxStep_num);
	mxDestroyArray(mxMode);
	mxDestroyArray(mxdt);
	mxDestroyArray(mxDEP_LOW);
	mxDestroyArray(mxDEP_HIGH);
	mxDestroyArray(mxRT_dep);
	mxDestroyArray(mxT_end);
	mxDestroyArray(mxControllerTypeID);
	
	engEvalString(ep,"diary MPC_Matlab.log");
	
	engClose(ep);
	fout_MPC3.close();
}
// ----------------------------------------------------------------------
double MPC3::update(struct lattice_state iLState,
					const double P,
					const double T)
{
	int i;
	char msgCommand[1024];
	msgCommand[0] = '\0';
	
	mxArray *mxW = NULL;
	double  *pW;
	mxArray *mxFval = NULL;
	double  *pFval;
	mxArray *mxHopt = NULL;
	double  *pHopt;
	mxArray *mxDopt = NULL;
	double  *pDopt;
	mxArray *mxR2opt = NULL;
	double  *pR2opt;
	mxArray *mxVarR2opt = NULL;
	double  *pVarR2opt;
	
	// 1. Pass variable from C to Matlab -------------------------------------
	memcpy(alphas,iLState.alphas,MODE*sizeof(double));
	memcpy(betas,iLState.betas,MODE*sizeof(double));
	ht			= iLState.H;
	(*pHt)		= iLState.H;
	R2			= iLState.R2;
//	(*pRough) = iLState.rough;
	g_dens		= iLState.SOR;
	(*pG_dens)	= iLState.SOR;
	(*pW0)		= dep_rate;
	(*pTime)	= time;

	engPutVariable(ep,"Alphas",mxAlphas);
	engPutVariable(ep,"Betas",mxBetas);
	engPutVariable(ep,"Ht",mxHt);
//	engPutVariable(ep,"Rough",mxRough);
	engPutVariable(ep,"G_dens",mxG_dens);
	engPutVariable(ep,"W0",mxW0);
	engPutVariable(ep,"Time",mxTime);

	engEvalString(ep,"In.alpha = Alphas");
	engEvalString(ep,"In.beta = Betas");
	engEvalString(ep,"In.rho = G_dens");
	engEvalString(ep,"In.R2 = R2");
	engEvalString(ep,"In.h = Ht");
	engEvalString(ep,"In.t = Time");

	// 2. Solve the optimization problem in Matlab ----------------------------
	sprintf(msgCommand,"[W,info,Settings] = MPC1xx_update(Settings,In,W0)");
	try{
		engEvalString(ep,msgCommand);
	}
	catch(const char *s) // If there is any error, report 
	{
		cout << "\n" << msgCommand << "\n";
		cout << "\n" << MatlabOutputBuffer << "\n";
		engEvalString(ep,"display(State)");
		cout << "\n" << MatlabOutputBuffer << "\n";
	}

	// 3. Get variables back to C----------------------------------------
	// cout << "Pass variables from Matlab to C";
	engEvalString(ep,"Uopt = info.Uopt");
	mxW 	= engGetVariable(ep,"Uopt");
	pW		= mxGetPr(mxW);
	for(i=0;i<Step_num;i++)
	{
		v_dep_rate[i+1] = pW[i];
	}
	dep_rate = pW[0];
	mxDestroyArray(mxW);
	
	// 4. Save diagnostic information ----------------------------------
	engEvalString(ep,"cost = info.cost");
	mxFval 	= engGetVariable(ep,"cost");
	pFval   = mxGetPr(mxFval);
	MPC_diag.cost    = pFval[0];
	mxDestroyArray(mxFval);
	engEvalString(ep,"Hopt = info.Hopt");
	mxHopt  = engGetVariable(ep,"Hopt");
	pHopt   = mxGetPr(mxHopt);
	engEvalString(ep,"Dopt = info.Dopt");
	mxDopt  = engGetVariable(ep,"Dopt");
	pDopt   = mxGetPr(mxDopt);
	engEvalString(ep,"R2opt = info.R2opt");
	mxR2opt = engGetVariable(ep,"R2opt");
	pR2opt  = mxGetPr(mxR2opt);
	engEvalString(ep,"varR2opt = info.varR2opt");
	mxVarR2opt = engGetVariable(ep,"varR2opt");
	pVarR2opt  = mxGetPr(mxVarR2opt);

	for(i=0;i<Step_num;i++)
	{
		MPC_diag.Hopt[i]  = pHopt[i];
		MPC_diag.Dopt[i]  = pDopt[i];
		MPC_diag.R2opt[i] = pR2opt[i];
		MPC_diag.varR2opt[i] = pVarR2opt[i];
	}
	mxDestroyArray(mxHopt);
	mxDestroyArray(mxDopt);
	mxDestroyArray(mxR2opt);
	mxDestroyArray(mxVarR2opt);

	time += t_int_ctrl;

	// 5. Write to log file --------------------------------------------
	if(t_idx<t_node)
	{
		W_sum[t_idx] += dep_rate;
		W2_sum[t_idx] +=  dep_rate*dep_rate;	
	}

	if(Detail ==1)
		log_detail();
	// --------------------------------------------------------------
	t_idx++;
	return dep_rate;
}
// ----------------------------------------------------------------------
void MPC3::reset()
{
	ht		= 0.0;
	R2      = 0.0;
	g_dens	= 0.0;	
	dep_rate	= DepRate_ini;	
	time		= 0.0;
	t_idx		= 0;
	
	W_sum[t_idx] += dep_rate;
	W2_sum[t_idx] +=  dep_rate*dep_rate;

	t_idx ++;
	Repeated += 1;

	engEvalString(ep,"State.h = 0");
	engEvalString(ep,"State.rho = 0");
	engEvalString(ep,"State.meanAlpha2= zeros(1,Settings.m)");
	engEvalString(ep,"State.meanBeta2 = zeros(1,Settings.m)");
	engEvalString(ep,"State.meanR2    = 0");
	engEvalString(ep,"State.varAlpha2 = zeros(1,Settings.m)");
	engEvalString(ep,"State.varBeta2  = zeros(1,Settings.m)");
	engEvalString(ep,"State.varR2     = 0");
}
